\documentclass[letter,10pt]{article}

\usepackage[utf8]{inputenc} % allow utf-8 input
\usepackage[T1]{fontenc}    % use 8-bit T1 fonts
\usepackage[colorlinks=true]{hyperref}       % hyperlinks
\usepackage{url}            % simple URL typesetting
\usepackage{booktabs}       % professional-quality tables
\usepackage{amsfonts,amsmath,amsthm,amssymb,gensymb}       % blackboard math symbols
\usepackage{microtype}      % microtypography
\usepackage{nicefrac}       % compact symbols for 1/2, etc.
\usepackage{color}
\usepackage{lmodern}
\usepackage{multicol}
\usepackage[margin=1.0in]{geometry}

\newcommand{\SE}[1]{ \mathrm{SE(#1)} }
\newcommand{\SO}[1]{ \mathrm{SO(#1)} }
\newcommand{\real}{\mathbb{R}}
\newcommand{\refgroup}{\mathrm{ref}}
\newcommand{\Exp}{\mathrm{Exp}}
\newcommand{\Log}{\mathrm{Log}}
% \newcommand{\asym}[1]{{\lbrack #1\rbrack}^\wedge{}}
\newcommand{\asym}[1]{{\lbrack #1\rbrack}_\times{}}
% \newcommand{\asym}[1]{\widehat{\lbrack #1\rbrack}}

\newtheorem{theorem}{Theorem}[section]
\newtheorem{prop}{Proposition}[section]

\title{An On Manifold Design of EKF-based VIO:\\
Implementation Notes of XIVO}
\author{Xiaohan Fei\\
\texttt{feixh@cs.ucla.edu}}
\begin{document}

\maketitle

% \begin{abstract}
% \end{abstract}

\section{Model}
% We simply layout the structure of this manuscript here.
% Section~\ref{sect-model} presents the model used in EKF Visual-Inertial SLAM: from an idealized one to an implementation-friendly one by adding more and more realistic considerations. 
% Section~\ref{sect-implementation} gives some implementation details including state representation, prediction \& update steps and initialization. 
% Section~\ref{sect-discussion} lists some potential improvements. 
% In the Appendix~\ref{sect-appendix}, we derive the analytical form of some Jacobians useful for implementation.

\subsection{Idealized model}
The model is the same as the one described in Section 2.1 of~\cite{jonesS10IJRR} of which the major results are briefly reviewed here. An idealized model to generate the sensed inertial $y_{imu}$ and visual data $y^i$ is given by:

\begin{equation}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{sb}^b(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=R_{sb}(t)v_{sb}^b(t), &T_{sb}(0)=0\\
\dot{v}_{sb}^b(t)=-\asym{\omega_{sb}^b(t)}v_{sb}^b(t)+\alpha_{sb}^b(t)\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t)=\pi\big(R_{sb}^\top(t)(X_0^i-T_{sb}(t))\big)\\
y_{imu}(t)=
\begin{bmatrix}
 \omega_m(t)\\
 \alpha_m(t)
\end{bmatrix}
\doteq
\begin{bmatrix}
	    \omega_{sb}^b(t)\\
	    \alpha_{sb}^b(t)-R_{sb}^\top(t)\gamma
           \end{bmatrix}.
\end{cases}
\label{eq-ideal-model}
\end{equation}

\begin{itemize}
 \item 
$R_{sb}(t) \in \SO{3}$ and $T_{sb}(t) \in \real^3$ are the body to spatial frame rotation and translation, which form $g_{sb}(t)\doteq (R_{sb}(t), T_{sb}(t)) \in \SE{3}$ the rigid body transformation from the body frame to the spatial frame. We adopt the convention in~\cite{maSKS} for the subscripts, where $g_{ab}$ denotes a transformation from reference frame $b$ to $a$, such that for a point $X$ represented in $b$, $g_{ab} X$ brings it to frame $a$. \footnote{In other words, $T_{sb}$ is the position of the origin of the body frame with respect to the spatial frame with coordinates resolved in the spatial frame and $R_{sb}$ transforms coordinates resolved in the body frame to coordinates resolved in the spatial frame by multiplying on the left: $x^s = R_{sb}x^b$.}

\item
$\omega_{sb}^b(t), v_{sb}^b(t)\in \real^3$ are the body to spatial rotational and linear velocity measured in the body frame. $\alpha_{sb}^b(t)$ is the body to spatial linear acceleration measured in body frame. \footnote{In other words, they are, respectively, the rotational velocity, linear velocity, and linear acceleration of the body frame with respect to the spatial frame with coordinates resolved in the body frame.}

\item
The ``hat'' operator $\hat{\cdot}$ (or $\asym{\cdot}$ when the operand is an expression instead of a single variable) maps a vector $v \doteq [v_1, v_2, v_3]^\top \in \real^3$ to a skew-symmetric matrix 
$$
\bar{v}=
\asym{v}=
\begin{bmatrix}
    0 & -v_3 & v_2 \\
    v_3 & 0 & -v_1 \\
    -v_2 & v_1 & 0
\end{bmatrix}
\doteq v_1 E_1 + v_2 E_2 + v_3 E_3$$ 
where $\{E_1, E_2, E_3\}$ forms a basis of ${\it so}(3)$ -- the Lie algebra of $\SO{3}$:

\begin{equation}
E_1=
\begin{bmatrix}
0 & 0 & 0 \\
0 & 0 & -1 \\
0 & 1 & 0
\end{bmatrix}
\quad
E_2=
\begin{bmatrix}
0 & 0 & 1 \\
0 & 0 & 0 \\
-1 & 0 & 0
\end{bmatrix}
\quad
E_3=
\begin{bmatrix}
0 & -1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 0
\end{bmatrix}.
\end{equation}


\item
$X_0^i \in \real^3, i=1\cdots N$ denotes the set of static 3-D points in the spatial frame, which have trivial dynamics.

\item
$y^i(t) \in \real^2$ is the measurement of point $i$ at time $t$, which is the projection of the 3-D point in our case.

\item
$y_{imu}(t)$ is the inertial measurement, which consists of the instantaneous rotational velocity and linear acceleration measured in the body frame.

\item
$\gamma$ is gravity in spatial frame.

\item
$\pi: \real^3 \mapsto \real^2$ is the perspective projection.

\end{itemize}

\subsection{From idealized model to a realistic one}
Define new linear velocity $v_{sb}\doteq R_{sb}v_{sb}^b$ and then we have $\dot{T}_{sb}(t)=v_{sb}(t)$. Define $\dot{v}_{sb}(t)\doteq \alpha_{sb}(t)$ and then $\alpha_{sb}=R_{sb}\alpha_{sb}^b$. For consistency, also define $\omega_{sb}\doteq \omega_{sb}^b$. The model becomes:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{sb}(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)=\alpha_{sb}(t)\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t)=\pi\big(R_{sb}^\top(t)(X_0^i-T_{sb}(t))\big)\\
\begin{bmatrix}
 \omega_m(t)\\
 \alpha_m(t)
\end{bmatrix}
\doteq
\begin{bmatrix}
	    \omega_{sb}(t)\\
	    R_{sb}^\top(t) \big(\alpha_{sb}(t)-\gamma \big)
           \end{bmatrix}
+           
           \begin{bmatrix}
           b_g\\
           b_a
           \end{bmatrix}.
\end{cases}
\end{equation*}

It is common practice to treat inertial measurements as inputs to the system (driving signals), thus we substitute $\omega_{sb}$ and $\alpha_{sb}$ with the actual inertial measurements:
\begin{equation*}
\begin{cases}
\omega_{sb}(t) = \omega_{m}-b_g\\
\alpha_{sb}(t) = R_{sb}(t)\big( \alpha_{m} - b_a\big) + \gamma
\end{cases}
\end{equation*}

and now the model is:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{m}(t)-b_g(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)= R_{sb}(t)\big( \alpha_m(t) - b_a(t) \big) + \gamma\\
\dot b_g(t)=0, & b_g(0)=\omega_{bias}^{\text{calib}}\\
\dot b_a(t)=0, & b_a(0)=\alpha_{bias}^{\text{calib}}\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t)=\pi\big(R_{sb}^\top(t)(X_0^i-T_{sb}(t))\big).
\end{cases}
\end{equation*}

In the above model, we assume the camera frame coincides with the body frame, which usually is not true. We need to insert the camera to body transformation $g_{bc}\doteq (R_{bc}, T_{bc}) \in \SE{3}$, which is from an off-line calibration procedure, into the visual measurement process:

\begin{equation}
\begin{aligned}
y^i(t) 
&= \pi \Big( g_{bc}^{-1} g_{sb}^{-1}(t)X_0^i \Big)\\
&= \pi \Big(R_{bc}^\top \Big( R_{sb}^\top(t)\big( X_0^i - T_{sb}(t)\big)-T_{bc} \Big)\Big).
\end{aligned}
\label{eq-vismeas}
\end{equation}

This camera to body alignment is usually available an off-line calibration procedure. However, high precision can be obtained if one treats the alignment states as unknown constants and estimate them on-line along with the ego-motion~\cite{li2012improving}. The alignment states have trivial dynamics since they are essentially constants:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{m}(t)-b_g(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)= R_{sb}(t)\big(\alpha_{m}(t) - b_a(t) \big) + \gamma\\
\dot b_g(t)=0, & b_g(0)=\omega_{bias}^{\text{calib}}\\
\dot b_a(t)=0, & b_a(0)=\alpha_{bias}^{\text{calib}}\\
\dot{R}_{bc}(t)=0, &R_{bc}(0)=R_{bc}^{\text{calib}}\\
\dot{T}_{bc}(t)=0, &T_{bc}(0)=T_{bc}^{\text{calib}}\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t) = \pi \Big(R_{bc}^\top(t) R_{sb}^\top(t)\big( X_0^i - T_{sb}(t)\big)-T_{bc}(t) \Big).
\end{cases}
\end{equation*}

Also, the gravity is assumed perfectly known, which is not true in practice -- at least one needs to estimate the orientation of gravity if not both the magnitude and the orientation. Let $R_g(t)$ be the orientation correction of the gravity, which is $R_g(0)=I$ if we initialize the body frame to be gravity aligned:
\begin{equation}
 R_{sb}(0)\big( \alpha_{m}(0)-b_a(0)\big) + R_g(0) \gamma = 0.
\end{equation}
The above equation holds if one puts the platform still for a certain amount of time during initialization, typically several seconds. One can either use one good sample of inertial measurements or average all samples during the stationary period to obtain $\alpha_{m}(0)$ and solve for $R_{sb}(0)$, whose solution is $R_{sb}^{\text{init}}$. The model is:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{m}(t)-b_g(t)}, &R_{sb}(0)=R_{sb}^{\text{init}}\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)= R_{sb}(t)\big( \alpha_{m}(t) - b_a(t) \big) + R_g(t) \gamma\\
\dot b_g(t)=0, & b_g(0)=\omega_{bias}^{\text{calib}}\\
\dot b_a(t)=0, & b_a(0)=\alpha_{bias}^{\text{calib}}\\
\dot{R}_{bc}(t)=0, &R_{bc}(0)=R_{bc}^{\text{calib}}\\
\dot{T}_{bc}(t)=0, &T_{bc}(0)=T_{bc}^{\text{calib}}\\
\dot{R}_g(t)=0, &R_g(0) = I\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t) = \pi \Big(R_{bc}^\top(t) R_{sb}^\top(t)\big( X_0^i - T_{sb}(t)\big)-T_{bc}(t) \Big).
\end{cases}
\end{equation*}

\section{Error state}
\label{sect-model}

Adding noise to the measurement model and dropping the time variable we obtain the following model:

System dynamics:
\begin{equation}
 \begin{cases}
 \dot R = R \asym{\omega_m-b_g-n_g}\\
 \dot T = v\\
 \dot v = R(a_m-b_a-n_a) + R_g \gamma \\
 \dot b_g = 0 \\
 \dot b_a = 0 \\
 \dot R_{bc} = 0 \\
 \dot T_{bc} = 0
 \end{cases}
\end{equation}

Each component can be written as the composition of a nominal state and an error state. For variables defined in Euclidean space the composition is the usual addition such as $T=\bar T + \tilde T$; for rotation, the composition is defined as $R=\bar R R(\tilde \omega)$ where $R(\tilde \omega)\doteq \exp(\asym{\tilde \omega})$ maps the error vector $\tilde \omega \in \real^3$ to a rotation matrix $R(\tilde \omega) \in \SO{3}$. The exponential map can be approximated as follows when the magnitude of $\tilde \omega$ is small:

\begin{equation}
R(\tilde \omega) \doteq \exp(\asym{\tilde \omega})= I + \asym{\tilde \omega}
\end{equation}


Derivation of nominal and error state system dynamics is trivial for translation:
\begin{equation}
\begin{cases}
\dot{\bar T} &= \bar v\\
\dot{\tilde T} &= \tilde v
\end{cases}
\end{equation}

For rotation, it's more involved:

\begin{equation}
 \begin{aligned}
 \dot R &= R\asym{\omega_m-b_g-n_g} \\
 \frac{d}{dt}\bar R  R(\tilde \omega) &= \bar R R(\tilde \omega) \asym{\omega_m - (\bar b_g+\tilde b_g) - n_g} \\
 \frac{d}{dt}\bar R  (I+ \asym{\tilde\omega}) &= \bar R (I+ \asym{\tilde\omega}) \asym{(\omega_m - \bar b_g) + (-\tilde b_g-n_g)} \\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
 &= \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{(\omega_m-\bar b_g) + (-\tilde b_g - n_g)}\\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
 &= \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\omega_m-\bar b_g} + \bar R \asym{\tilde \omega}\asym{-\tilde b_g - n_g}\\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
 &\approx \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\omega_m-\bar b_g}\\
 \end{aligned}
 \label{eq-rotation-derivation}
\end{equation}

$\asym{\tilde \omega}\asym{-\tilde b_g - n_g}$ is the product of two error states/one error state and one noise term, which approximates zero and thus is dropped.

Now we have the evolving equation of the nominal \& error rotation state:
\begin{equation}
\begin{aligned}
 \dot{\bar R} &= \bar R\asym{\omega_m - \bar b_g} = \bar R \asym{\bar \omega}\\
\dot{\bar R}\asym{\tilde\omega} + \bar R \asym{\dot{\tilde \omega}} 
&= \bar R \asym{-\tilde b_g - n_g} + \bar R\asym{\tilde\omega}\asym{\bar\omega}
\end{aligned}
\end{equation}
where $\bar \omega \doteq \omega_m - \bar b_g$.

Recall the identity $\dot R=R \asym{\omega}$, and thus $R^\top \dot R = \asym{\omega}$. 

We left-multiply both sides of the last equation by $\bar R^\top$, and use the identity $\bar R^\top\dot{\bar R}=\asym{\bar\omega}$
\begin{equation}
\begin{aligned}
 \asym{\bar \omega}\asym{\tilde \omega} + \asym{\dot{\tilde\omega}} &= \asym{-\tilde b_g - n_g} + \asym{\tilde\omega}\asym{\bar\omega}\\
 \asym{\dot{\tilde\omega}} &= \asym{-\tilde b_g - n_g} 
 + \big(\asym{\tilde\omega}\asym{\bar\omega} - \asym{\bar\omega}\asym{\tilde\omega}\big) \\
 \asym{\dot{\tilde\omega}} &= \asym{-\tilde b_g - n_g} + \asym{\big(\tilde\omega\times\bar\omega\big)} \quad\text{(recall} \asym{a\times b}=\asym{a}\asym{b}-\asym{b}\asym{a}\text{)}\\
\dot{\tilde\omega} &= -\tilde b_g - n_g + \tilde\omega\times\bar\omega \quad\text{(}\asym{\cdot} \text{ is linear)} \\
\dot{\tilde\omega} &= -\tilde b_g - n_g + \asym{\tilde\omega}\bar\omega \quad\text{(}a\times b=\asym{a}b \text{)}\\
\dot{\tilde\omega} &= -\tilde b_g - n_g - \asym{\bar\omega}\tilde\omega \quad\text{(}\asym{\cdot} \text{ is anti-commutative)}\\
\end{aligned}
\end{equation}

To summarize:
\begin{equation}
\begin{cases}
 \dot{\bar R} &= \bar R \asym{\bar \omega}\\
\dot{\tilde\omega} &= - \asym{\bar\omega}\tilde\omega -\tilde b_g - n_g.
\end{cases}
\end{equation}

Last let's work on velocity term:

\begin{equation}
\begin{aligned}
 \dot v &= R(a_m-b_a-n_a) + R_g \gamma \\
 \dot{\bar v} + \dot{\tilde v} &= \bar R R(\tilde \omega) (a_m-(\bar b_a+\tilde b_a)-n_a) + \bar R_g(I + \asym{\tilde\omega_g}) \gamma \\
 \dot{\bar v} + \dot{\tilde v} &= \bar R (I+\asym{\tilde\omega}) (a_m-(\bar b_a+\tilde b_a)-n_a) + \bar R_g \gamma + \bar R_g \asym{\tilde\omega_g} \gamma\\
 \dot{\bar v} + \dot{\tilde v} &= \bar R (I+\asym{\tilde\omega}) \big( (a_m-\bar b_a) - (\tilde b_a+n_a) \big) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g\\
 \dot{\bar v} + \dot{\tilde v} &= \bar R (a_m-\bar b_a) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}(a_m-\bar b_a)- \bar R\asym{\tilde\omega}(\tilde b_a+n_a) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g \\
 \dot{\bar v} + \dot{\tilde v} &\approx \bar R (a_m-\bar b_a) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}(a_m-\bar b_a) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g\\
\end{aligned}
 \label{eq-velocity-derivation}
\end{equation}

where nominal and error state equation can be defined as
\begin{equation}
\begin{cases}
\dot{\bar v} &= \bar R\bar a + \bar R_g \gamma\\
\dot{\tilde v} &= -\bar R(\tilde b_a + n_a) + \bar R \asym{\tilde\omega}\bar a - \bar R_g \asym{\gamma} \tilde\omega_g 
\end{cases}
\end{equation}
where we have defined $\bar a \doteq a_m - \bar b_a$.

Note that $\asym{a}b=a\times b=-b\times a=-\asym{b} a$, we can write last equation 

\begin{equation}
\dot{\tilde v}=-\bar R(\tilde b_a + n_a) - \bar R\asym{\bar a} \tilde \omega- \bar R_g \asym{\gamma} \tilde \omega_g.
\end{equation}

To summarize:
\begin{equation}
\begin{cases}
\dot{\bar R} &= \bar R\asym{\bar \omega}\\
\dot{\bar T} &= \bar v\\
\dot{\bar v} &= \bar R \bar a + \bar R_g \gamma
\end{cases}
\label{eq:nominal-dynamics}
\end{equation}

and

\begin{equation}
\begin{bmatrix}
\dot{\tilde \omega} \\
\dot{\tilde T}\\
\dot{\tilde v}\\
\dot{\tilde b}_g\\
\dot{\tilde b}_a\\
\dot{\tilde \omega}_g
\end{bmatrix}
= 
\begin{bmatrix}
-\asym{\bar\omega} & 0_{3\times3} & 0_{3\times3} & -I_{3\times3} & 0_{3\times3} & 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}\\
-\bar R\asym{\bar a} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & -\bar R& -\bar R_g \asym{\gamma}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}
\end{bmatrix}
\begin{bmatrix}
\tilde \omega \\
\tilde T\\
\tilde v\\
\tilde b_g\\
\tilde b_a\\
\tilde \omega_g
\end{bmatrix}
+
\begin{bmatrix}
-I_{3\times3} & 0_{3\times3}& 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & -\bar R & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & I_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3}& I_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
\end{bmatrix}
\begin{bmatrix}
 n_g\\
 n_a\\
 n_{b_g}\\
 n_{b_a}\\
\end{bmatrix}
\end{equation}

For convenience, define $\tilde X \doteq [\tilde \omega, \tilde T, \tilde v, \tilde b_g, \tilde b_a, \tilde\omega_g]^\top \in \real^{18}$ and equation above reads
\begin{equation}
\dot{\tilde X} = F \tilde X + G n_{imu}
\end{equation}

\subsection{Online calibration of IMU intrinsics}
IMU intrinsics (scaling and misalignment factors) are typically obtained via an offline calibration procedure. Nevertheless, we can also treat them as unknown constants and calibrate them online by incorporating them into the state with small initial covariance. 

Let $C_a\in\real^{3\times 3}$ and $C_g\in\real^{3 \times 3}$ be the calibration matrix of the accelerometer and gyroscope respectively.~\footnote{To be precise, $C_a$ and $C_g$ live on some manifolds since they have some intrinsic structures. For instance, $C_a$ is composed of a scaling matrix which has positive entries on the diagonal and zeros otherwise, and an orthonormal misalignment matrix.}

\subsubsection{Rotation}
Recall the last line of Eq.~\eqref{eq-rotation-derivation}
$$
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}} \approx \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\omega_m-\bar b_g}
$$
To incorporate the gyro calibration matrix $C_g$, substitute the term $\omega_m$ with $C_g\omega_m$:
$$
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}} \approx \bar R\asym{C_g\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{C_g\omega_m-\bar b_g},
$$
and write the gyro calibration in terms of its nominal and error state $C_g=\bar C_g + \tilde C_g$:
\begin{align}
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
  &\approx \bar R\asym{(\bar C_g + \tilde C_g)\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{(\bar C_g + \tilde C_g)\omega_m-\bar b_g}\\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
  &\approx 
  \bar R\asym{\bar C_g\omega_m-\bar b_g} 
  + \bar R\asym{\tilde C_g\omega_m} \\
  & \quad + \bar R\asym{-\tilde b_g - n_g}  \\
  & \quad + \bar R \asym{\tilde \omega}\asym{\bar C_g\omega_m-\bar b_g}
  + \bar R \asym{\tilde \omega}\asym{\tilde C_g\omega_m} \text{ (2nd term is high-order)}\\
  &\approx
  \bar R\asym{\bar C_g\omega_m-\bar b_g} + \bar R\asym{\tilde C_g\omega_m}
  + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\bar C_g\omega_m-\bar b_g}
\end{align}

Now, the nominal state evolves as
$$
\dot{\bar R} = \bar R\asym{\bar C_g \omega_m - \bar b_g}
$$
resulting the identity $\bar R^\top \dot{\bar R} = \asym{\bar C_g \omega_m - \bar b_g}$ which can be used to simplify the rest of the equation:
$$
\dot{\bar R}\asym{\tilde\omega} + \bar R\asym{\dot{\tilde\omega}} =
  \bar R\asym{\tilde C_g\omega_m} + \bar R\asym{-\tilde b_g - n_g}  
    + \bar R \asym{\tilde \omega}\asym{\bar C_g\omega_m-\bar b_g}.
$$
Let $\bar\omega=\bar C_g \omega_m - \bar b_g$, and left-multiple $\bar R^\top$ on both sides of the equation above to cancel out $\bar R$
\begin{align}
\asym{\bar\omega}\asym{\tilde\omega} + \asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}  
    + \asym{\tilde \omega}\asym{\bar\omega} \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}  
    + \big(\asym{\tilde\omega}\asym{\bar\omega}-\asym{\bar\omega}\asym{\tilde\omega}\big) \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}  
    + \asym{\tilde\omega\times\bar\omega} \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}  
    - \asym{\bar\omega\times\tilde\omega} \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}  
    - \asym{\asym{\bar\omega}\tilde\omega} \\
\dot{\tilde\omega} &= \tilde C_g\omega_m - \tilde b_g - n_g - \asym{\bar\omega}\tilde\omega
\end{align}
leading to the error state equation of $\tilde\omega$:
$$
\dot{\tilde\omega} = -\asym{\bar\omega}\tilde{\omega} + \tilde{C_g} \omega_m - \tilde b_g - n_g
$$


\subsubsection{Velocity}

Recall the last line of Eq.~\eqref{eq-velocity-derivation}
$$
 \dot{\bar v} + \dot{\tilde v} \approx \bar R (a_m-\bar b_a) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}(a_m-\bar b_a) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g.
$$

To consider the accelerometer calibration, substitute $\alpha_m$ with $C_a\alpha_m=(\bar C_a+\tilde C_a)\alpha_m$:
$$
 \dot{\bar v} + \dot{\tilde v} \approx \bar R \big((\bar C_a + \tilde C_a)a_m-\bar b_a\big) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}\big((\bar C_a + \tilde C_a) a_m-\bar b_a\big) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g.
$$

Nominal state evolves as $\dot{\bar v} = \bar R\bar\alpha + \bar R_g \gamma$, where $\bar\alpha\doteq\bar C_a \alpha_m - \bar b_a$.

The rest of the equation can be further simplified
\begin{align}
\dot{\tilde v} &= \bar R \tilde C_a a_m - \bar R(\tilde b_a + n_a) + \bar R\asym{\tilde\omega}\big(\bar\alpha  + \tilde C_a \alpha_m\big) - \bar R_g\asym{\gamma}\tilde\omega_g \\
\dot{\tilde v} &= \bar R \tilde C_a a_m - \bar R(\tilde b_a + n_a) - \bar R\asym{\bar\alpha}\tilde\omega - \bar R_g \asym{\gamma}\tilde\omega_g
\end{align}
by dropping 2nd-order residual terms and using the identity $\asym{a}b=-\asym{b}a$.




\subsection{Integration of motion}

With coming inertial measurements, the motion of the sensor platform can be obtained by numerical integration of Eq.~\eqref{eq:nominal-dynamics} (there are numerous textbooks on this topic, for instance, \cite{ascher1998computer}). As pointed out in \cite{ascher1998computer}, Fehlberg's, and Prince-Dormand's embedded Runge-Kutta methods are among the most popular ones. Some reference implementations can be found at \url{http://www.mymathlib.com/diffeq/embedded_runge_kutta/}.

To propagate the covariance matrix of the error state, we follow \cite{mourikis2007multi} where the covariance matrix is partitioned as follows:

\begin{equation}
    P_{k|k} = 
    \begin{bmatrix}
        P_{k|k}^{(1)} & P_{k|k}^{(2)} \\
        P_{k|k}^{(2)\top} & P_{k|k}^{(3)}
    \end{bmatrix}
    \label{eq:covariance-partition}
\end{equation}
where $P_{k|k}^{(1)}$ is the $18\times 18$ covariance matrix of the evolving motion state, $P_{k|k}^{(3)}$ corresponds to the structure state (including pose of groups and position of features, detailed later), and $P_{k|k}^{(2)}$  is the correlation between the errors in the motion state and the structure state. The covariance matrix of the propagated state is given by:

\begin{equation}
    P_{k+1|k} =
    \begin{bmatrix}
        P_{k+1|k}^{(1)} & \Phi(t_{k + 1}, t_k) P_{k|k}^{(2)} \\
        P_{k|k}^{(2)\top}\Phi(t_{k + 1}, t_k)^\top & P_{k|k}^{(3)}  \\
    \end{bmatrix}
    \label{eq:covariance-propagation}
\end{equation}

where $P_{k+1|k}^{(1)}$ is computed by numerical integration of the following Lyapunov equation for the time interval $(t_k, t_{k+1})$ with initial condition $P=P_{k|k}^{(1)}$:

\begin{equation}
    \dot{P} = F P + P F^\top + G Q_{imu} G^\top.
\label{eq:lyapunov}
\end{equation}

The state transition matrix $\Phi(t_{k+1}, t_k)$ is obtained by numerical integration of the following matrix differential equation over $(t_k, t_{k+1})$ with initial condition $\Phi(t_k, t_k)=I_{18\times 18}$:

\begin{equation}
    \dot{\Phi}(\tau, t_k) = F \Phi(\tau, t_k), \quad\tau \in [t_k, t_{k+1}].
\end{equation}

\section{State augmentation}
Once the pose estimate of a newly arrived image is available, its camera-to-spatial transformation $g_{sc} \in \SE{3}$ is inserted into the state. $g_{sc}$ is the composed of the body-to-spatial transformation $g_{sb} \in \SE{3}$ and the camera-to-body alignment $g_{bc} \in \SE{3}$. Written in components
\begin{equation}
\begin{cases}
R_{sc} &= R_{sb} R_{bc}\\
T_{sc} &= R_{sb} T_{bc} + T_{sb}
\end{cases}
\end{equation}

\subsection{Rotation}
With error state representation, the rotation part is:
\begin{equation}
 \begin{aligned}
R_{sc} &= R_{sb} R_{bc} \\
 (\bar R_{sc} + \bar R_{sc} \asym{\tilde\omega_{sc}}) &= 
 (\bar R_{sb} + \bar R_{sb} \asym{\tilde\omega_{sb}}) (\bar R_{bc} + \bar R_{bc} \asym{\tilde\omega_{bc}}) \\
 \bar R_{sc} + \bar R_{sc}\asym{\tilde \omega_{sc}} &\approx \bar R_{sb} \bar R_{bc} 
 + \bar R_{sb} \asym{\tilde \omega_{sb}} \bar R_{bc} + \bar R_{sb} \bar R_{bc} \asym{\tilde\omega_{bc}} \quad \text{ drop } \asym{\tilde\omega_{sb}}\asym{\tilde\omega_{bc}}
 \end{aligned}
\end{equation}

Group terms on both sides into nominal part and error-state part respectively:
\begin{equation}
\begin{cases}
\bar R_{sc} &= \bar R_{sb} \bar R_{bc}\\
\asym{\tilde \omega_{sc}} &= \bar R_{sc}^\top \bar R_{sb} \asym{\tilde\omega_{sb}} \bar R_{bc} + \bar R_{sc}^\top \bar R_{sb} \bar R_{bc} \asym{\tilde\omega_{bc}}
\end{cases}
\end{equation}
where the second equation can be further simplified as $\tilde \omega_{sc} = \bar R_{bc}^\top \tilde \omega_{sb} + \tilde \omega_{bc}$ by noticing the following:
\begin{itemize}
 \item 
$\bar R_{sc}^\top \bar R_{sb}=\bar R_{cb} = \bar R_{bc}^\top = \bar R_{bc}^{-1}$ 
\item
$R \asym{\omega} R^\top=\asym{R \omega} \quad \forall R \in \SO{3}, \omega \in \real^3$ (see appendix for a proof)
\end{itemize}

\subsection{Translation}
\begin{equation}
\begin{aligned}
T_{sc} &= R_{sb} T_{bc} + T_{sb} \\
\bar T_{sc} + \tilde T_{sc} &= (\bar R_{sb} + \bar R_{sb} \asym{\tilde \omega_{sb}}) (\bar T_{bc} + \tilde T_{bc}) + \bar T_{sb} + \tilde T_{sb} \\
\bar T_{sc} + \tilde T_{sc} &\approx \big(\bar R_{sb} \bar T_{bc} + \bar T_{sb} \big)
+ \bar R_{sb} \tilde T_{bc} + \bar R_{sb} \asym{\tilde \omega_{sb}} \bar T_{bc} + \tilde T_{sb} \quad \text{ drop } \bar R_{sb} \asym{\tilde \omega_{sb}} \tilde  T_{bc}
\end{aligned}
\end{equation}

Group terms into nominal and error-state equation and simplify:
\begin{equation}
\begin{cases}
 \bar T_{sc} &= \bar R_{sb} \bar T_{bc} + \bar T_{sb} \\
 \tilde T_{sc} &= \bar R_{sb} \tilde T_{bc} - \bar R_{sb} \asym{\bar T_{bc}} \tilde \omega_{sb} + \tilde T_{sb}
\end{cases}
\end{equation}



To summarize, the nominal equations for the augmented pose are
\begin{equation}
\begin{cases}
\bar R_{sc} &= \bar R_{sb} \bar R_{bc} \\
 \bar T_{sc} &= \bar R_{sb} \bar T_{bc} + \bar T_{sb}
\end{cases}
\end{equation}

and the error-state equations are
\begin{equation}
\begin{cases}
\tilde \omega_{sc} &= \bar R_{bc}^\top \tilde \omega_{sb} + \tilde \omega_{bc} \\
 \tilde T_{sc} &= \bar R_{sb} \tilde T_{bc} - \bar R_{sb} \asym{\bar T_{bc}} \tilde \omega_{sb} + \tilde T_{sb}
\end{cases}
\end{equation}

To augment the covariance matrix, build the Jacobian according to the error-state equation as $J \in \real^{6\times N}$ where the covariance matrix before augmentation is $P \in \real^{N\times N}$ then

\begin{equation}
 P \leftarrow 
 \begin{bmatrix}
  I_{N\times N}\\
  J
 \end{bmatrix}
 P
 \begin{bmatrix}
 I_{N\times N}\\
 J
 \end{bmatrix}^\top
\end{equation}

Note, if camera-to-body transformation is not being estimated, simply set its error state  to zero, i.e., $\tilde \omega_{bc}=0$ and $\tilde T_{bc}=0$ and the nominal state as the true state (calibrated offline), i.e., $\bar R_{bc}=R_{bc}$ and $\bar T_{bc}=T_{bc}$.

\subsection{State augmentation with $g_{sb}$}

Instead of $g_{sc}$, we can augment the state with body-to-spatial transformation $g_{sb}$ of the arriving image and share the camera-to-body alignement $g_{bc}$  among them. In this case, initialization of the error state is trivial: Simply duplicate the nominal state, the error state and the covariance block corresponding to $g_{sb}$ of the current state being estimated

\subsection{State augmentation with 3D points}

Simply duplicate the state and covariance of the depth subfilter (Sect.~\ref{sect-depth}) into the main state and covariance.


\section{Measurement update - MSCKF}
Assume the depth subfilter (Sect.~\ref{sect-depth}) estimates the feature state well whose coordinates in the spatial frame $X_s\in\real^3$ can be computed. For each group $g_{sc}(t)$ to which the feature is visible, the pixel coordinates read 

\begin{equation}
 x_p(t) = \pi(g_{sc}^{-1}(t)X_s) = \pi\big( R_{sc}^\top (X_s-T_{sc}) \big).
\end{equation}
Furthermore, if the state is augmented with camera-to-body pose $g_{sb}$ with shared camera-to-body alignemnt $ig_{bc}$ the visual measurements read

\begin{equation}
x_p(t) = \pi(g_{bc}^{-1}(t)g_{sb}^{-1}(t)X_s)=\pi\big(R_{bc}^\top (R_{sb}^\top(X_s-T_{sb}) - T_{bc})\big).
\end{equation}

In this section, we derive the error-state form of the measurement equations w/ and w/o the camera-to-body alignment incorporated into the augmented pose.  With the $g_{bc}$ incorporated into $g_{sc}$, we are assuming $g_{bc}$ is perfectly known after calibration before running the estimator, which is not realistic due to imperfection of the calibration procedure. With only $g_{sb}$ augmented to the state and the $g_{bc}$ shared among all the augmented poses, we will be able to perform online spatial calibration with the initial nominal value provided by the offline calibration.

\subsection{State augmentation with $g_{sc}$}

Taylor-expand the projection function
\begin{equation}
 \pi(\bar a + \tilde a) 
 = \pi(\bar a) + \frac{d \pi}{d a}|_{a=\bar a} \tilde a + o(\tilde a)
 \approx \pi(\bar a) + \pi'(\bar a) \tilde a.
\end{equation}

Therefore if we write $R_{sc}^\top(X_s-T_{sc})$ in the form of $\bar a+\tilde a$, we will the following nominal and error-state equations:

\begin{equation}
\begin{cases}
\bar x_p = \pi(\bar a)\\
\tilde x_p = \pi'(\bar a) \tilde a
\end{cases}
\end{equation}

\begin{equation}
\begin{aligned}
R_{sc}^\top(X_s-T_{sc}) 
&= (\bar R_{sc} + \bar R_{sc}\asym{\tilde \omega_{sc}})^\top (\bar X_s - \bar T_{sc} + (\tilde X_s - \tilde T_{sc})) \\
R_{sc}^\top(X_s-T_{sc}) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
&= (\bar R_{sc}^\top - \asym{\tilde \omega_{sc}}\bar R_{sc}^\top) (\bar X_s - \bar T_{sc} + (\tilde X_s - \tilde T_{sc})) \\
R_{sc}^\top(X_s-T_{sc}) 
&\approx \bar R_{sc}^\top(\bar X_s - \bar T_{sc}) 
+ \bar R_{sc}^\top(\tilde X_s - \tilde T_{sc})
- \asym{\tilde \omega_{sc}} \bar R_{sc}^\top(\bar X_s - \bar T_{sc})\\
R_{sc}^\top(X_s-T_{sc}) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
&\approx \bar R_{sc}^\top(\bar X_s - \bar T_{sc}) 
+ \bar R_{sc}^\top(\tilde X_s - \tilde T_{sc})
+ \asym{\bar R_{sc}^\top(\bar X_s - \bar T_{sc})} \tilde\omega_{sc}\\
\end{aligned}
\end{equation}

To summarize:

\begin{equation}
\begin{cases}
\bar x_p=\pi\big( \bar R_{sc}^\top (\bar X_s - \bar T_{sc})\big)\\
\tilde x_p=\pi'\big(\bar R_{sc}^\top(\bar X_s - \bar T_{sc})\big) 
\big( \bar R_{sc}^\top(\tilde X_s - \tilde T_{sc}) + \asym{\bar R_{sc}^\top(\bar X_s - \bar T_{sc})} \tilde \omega_{sc}\big)
\end{cases}
\end{equation}

\subsection{State augmentation with $g_{sb}$}
\label{sect-msckf-gsb}
Again, we need to write $R_{bc}^\top\big(R_{sb}^\top(X_s-T_{sb})-T_{bc})\big)$ in the form of $\bar a + \tilde a$:

\begin{equation}
\begin{aligned}
 & R_{bc}^\top\big(R_{sb}^\top(X_s-T_{sb})-T_{bc})\big) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 = & (\bar R_{bc} + \bar R_{bc}\asym{\tilde\omega_{bc}})^\top
 \big( (\bar R_{sb} + \bar R_{sb}\asym{\tilde\omega_{sb}})^\top (\bar X_s - \bar T_{sb}) + (\bar R_{sb} + \bar R_{sb}\asym{\tilde\omega_{sb}})^\top (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 = & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)
 \big( (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\bar X_s - \bar T_{sb}) + (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
= & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)
 (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\bar X_s - \bar T_{sb}) \\
 &+ (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^
 \top)(\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\tilde X_s - \tilde T_{sb}) \\
 &- (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)(\bar T_{bc} + \tilde T_{bc}) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 \approx & \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \asym{\tilde \omega_{bc}}\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \asym{\tilde \omega_{sb}} \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) \\
 &+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) \\
 &- \bar R_{bc}^\top \bar T_{bc} - \bar R_{bc}^\top \tilde T_{bc} + \asym{\tilde \omega_{bc}} \bar T_{bc} \quad\text{(drop high-order terms)}\\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 = & \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) 
 + \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc} 
 + \bar R_{bc}^\top \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})} \tilde \omega_{sb} \\
 &+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) \\
 &- \bar R_{bc}^\top \bar T_{bc} 
 - \bar R_{bc}^\top \tilde T_{bc} 
 - \asym{\bar R_{bc}^\top \bar T_{bc}}\tilde \omega_{bc}  \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
=& \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc} \quad\text{(nominal term)}\\
&+ \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc} 
+ \bar R_{bc}^\top \asym{ \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{sb}
 + \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) 
 - \bar R_{bc}^\top \tilde T_{bc} - \asym{\bar R_{bc}^\top \bar T_{bc}} \tilde \omega_{bc} \\
\end{aligned}
\end{equation}

To summarize:

\begin{equation}
\begin{cases}
\bar x_p &= \pi\big(\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc}\big) \\
\tilde x_p &= \pi'\big(\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc}\big) \\
    &\cdot 
    
    \big( 
    \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc}}\tilde \omega_{bc} 
+ \bar R_{bc}^\top \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{sb}
+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) 
- \bar R_{bc}^\top \tilde T_{bc}
    \big)
\end{cases}
\label{eq-msckf-gsb}
\end{equation}

\subsection{Online temporal calibration}

With the temporal offset $t_d$ in image timestamps, we need to bring the 3D point to time instant $t+t_d$ when the visual features are actually measured instead of time $t$ when the measurements are timestamped. The transformation reads $X_c = R_{bc}^\top\big(R_{sb}(t+t_d)^\top (X_s - T_{sb}(t+t_d)) - T_{bc}\big)$, where $T_{sb}\approx \bar T_{sb} + \bar v_{sb} \tilde t_d$,(2nd-order error terms dropped) and
$$
R_{sb}(t+t_d) \approx \bar R_{sb}(I+\asym{\tilde \omega_{sb}})(I + \asym{\bar\omega}\tilde t_d)=\bar R_{sb} + \bar R_{sb}\asym{\tilde \omega_{sb}} + \bar R_{sb}\asym{\bar\omega} \tilde t_d \quad\text{(drop 2nd-order error terms)}
$$
where $\bar R_{sb}\doteq \bar R_{sb}(t+\bar t_d)$ is the rotation propagated to time $(t+t_d)$, and $\bar \omega$ is the measured angular velocity.

Error terms involving $\tilde t_d$ in $X_c$ then reads
$$
\bar R_{bc}^\top \big(\bar R_{sb}\asym{\bar\omega}\tilde t_d)^\top (X_s-\bar T_{sb}) - \bar R_{bc}^\top \bar R_{sb}^\top \bar v_{sb} \tilde t_d
= -\bar R_{bc}^\top \big(\asym{\bar\omega}\bar R_{sb}^\top (X_s - \bar T_{sb}) + \bar R_{sb}^\top \bar v_{sb} \big)\tilde t_d.
$$

This residual block can be inserted to the second part of the measurement residual equation $\tilde x_p$ of Eq.~\eqref{eq-msckf-gsb}, i.e., 
$$
\tilde x_p=\pi'(\ldots)\cdot\big(\ldots
- \bar R_{bc}^\top \big(\asym{\bar\omega}\bar R_{sb}^\top (X_s- \bar T_{sb}) + \bar R_{sb}^\top \bar v_{sb} \big)\tilde t_d
\big)
$$

Note, if online IMU intrinsics calibration is enabled, $\bar \omega=C_g \omega_m - b_g$, where $C_g = \bar C_g + \tilde C_g$ and $b_g=\bar b_g + \tilde b_g$, and hence we need to expand $\tilde x_p$ w.r.t. the error state of both the IMU intrinsics $\tilde C_g$ and the bias $\tilde b_g$:

$$
\tilde x_p
= \pi'(\ldots)\cdot
\big(
    \ldots
    +\bar R_{bc}^\top\asym{\bar R_{sb}^\top (X_s - \bar T_{sb})} \bar t_d 
    ( \tilde C_g \omega_m - \tilde b_g)
\big)
$$







\section{Measurement update - EKF}

The derivation of the EKF measurement equation in error state form closely follows Sect.~\ref{sect-msckf-gsb}. In fact, we can write down the error state representation of $X_s\approx \bar{X}_s + \tilde{X}_s$ in terms of the feature parametrization (since the feature is in state now) and substitute each term into Eq.\eqref{eq-msckf-gsb}.

\subsection{Error state form of $X_s$}

Let's first express $X_s$ in error state form. Each 3D point is parametrized as $(x_c, y_c, \rho)$ where $\rho$ is the inverse depth, i.e., the coordinates of the 3D point represented in the its reference camera frame is $X_c = [x_c/\rho, y_c/\rho, 1/\rho]^\top$.

\begin{equation}
\begin{aligned}
X_s &= g_{sb}(t_r) g_{bc} X_c \quad\text{(let } g_{sb}(t_r)=[R_r|T_r] \text{)}\\
&= R_r(R_{bc} X_c + T_{bc}) + T_r \\ 
%&= R_r(R_{bc} (\bar X_c + \tilde X_c)
%+ T_{bc}) + T_r \quad\text{formally of } X_c = \bar X_c + \tilde X_c\\
&= (\bar R_r + \bar R_r \asym{\tilde \omega_r})(\bar R_{bc} + \bar R_{bc}\asym{\tilde \omega_{bc}})(\bar X_c + \tilde X_c) + (\bar R_r + \bar R_r \asym{\tilde \omega_r})(\bar T_{bc} + \tilde T_{bc}) + (\bar T_r + \tilde T_r) \\
&= \bar R_r \bar R_{bc} \bar X_c  + 
\bar R_r \bar R_{bc} \tilde X_c +
\bar R_r \bar R_{bc} \asym{\tilde\omega_{bc}} \bar X_c +
\bar R_r \asym{\tilde \omega_r} \bar R_{bc} \bar X_c \quad\text{(1st term, drop higher-order terms)}\\
&\quad + \bar R_r \bar T_{bc} + \bar R_r \tilde T_{bc} + \bar R_r \asym{\tilde\omega_r} \bar T_{bc} \quad\text{(2nd term, drop higher-order terms)}\\
&\quad + \bar T_r + \tilde T_r.
\end{aligned}
\end{equation}

To summarize:
\begin{equation}
\begin{cases}
\bar X_s &= \bar R_r \bar R_{bc} \bar X_c + \bar R_r \bar T_{bc} + \bar T_r\\
\tilde X_s &= \bar R_r \bar R_{bc} \tilde X_c - \bar R_r \bar R_{bc} \asym{\bar X_c} \tilde \omega_{bc} - \bar R_r \asym{\bar R_{bc} \bar X_c + \bar T_{bc}} \tilde \omega_r + \bar R_r \tilde T_{bc} + \tilde T_r.
\end{cases}
\end{equation}

Now we need to expand $X_c = [x_c/\rho, y_c/\rho, 1/\rho]^\top$ in error state form. Let's start with x-component (same procedure applies to y-component due to symmetry):
\begin{equation}
\begin{aligned}
\frac{x_c}{\rho}
&= \frac{\bar x_c + \tilde x_c}{\bar \rho_c + \tilde \rho_c} \\
&= (\bar x_c + \tilde x_c) \left (  \frac{\bar \rho_c - \tilde \rho_c}{\bar \rho_c^2 - \tilde \rho_c^2}  \right ) \\
&\approx (\bar x_c + \tilde x_c) \left (  \frac{\bar \rho_c - \tilde \rho_c}{\bar \rho_c^2}  \right ) \quad\text{(drop higher-order term in denominator)}\\
&= (\bar x_c + \tilde x_c)\left (\frac{1}{\bar \rho_c} - \frac{1}{\bar \rho_c^2}\tilde \rho_c \right )\\
&\approx \frac{\bar x_c}{\bar\rho_c} - \frac{\bar x_c}{\bar \rho_c^2} \tilde\rho_c + \frac{1}{\bar \rho_c} \tilde x_c \quad\text{(drop higher-order terms)}.
\end{aligned}
\end{equation}

\begin{equation}
\begin{cases}
\bar X_c &=
\frac{1}{\bar \rho_c}
\begin{bmatrix}
    \bar x_c\\
    \bar y_c\\
    1
\end{bmatrix} \\
\tilde X_c &=
\begin{bmatrix}
-\frac{\bar x_c}{\bar \rho_c^2} \tilde\rho_c + \frac{1}{\bar \rho_c} \tilde x_c \\
-\frac{\bar y_c}{\bar \rho_c^2} \tilde\rho_c + \frac{1}{\bar \rho_c} \tilde y_c \\
-\frac{1}{\bar \rho_c^2} \tilde \rho_c
\end{bmatrix} 
=
\begin{bmatrix}
    1/\bar\rho_c & 0 & -\bar x_c / \bar\rho_c^2\\
    0 & 1/\bar\rho_c & -\bar y_c / \bar\rho_c^2\\
    0 & 0 & -1 / \bar\rho_c^2\\
\end{bmatrix}
\begin{bmatrix}
    \tilde x_c \\
    \tilde y_c \\
    \tilde \rho_c
\end{bmatrix}
\end{cases}
\end{equation}
Substitute the above into $\bar X_s, \tilde X_s$, and we get the full measurement equation in error state form.







\section{Depth subfilter}
\label{sect-depth}
When a feature is first observed at time $t_r$ with pixel coordinates $[x_p(t_r) , y_p(t_r)]^\top\in\real^2$, we associate it with a reference group with camera pose $g_{ref}\doteq g_{sc}(t_r)$ and initialize its state as $x=[x_c, y_c, \rho]^\top \in \real^3$ where $\rho \in \real_+$ is the inverse depth initialized at a nominal distance to the image plane and $[x_c, y_c]^\top$ is the feature position in camera coordinates:
\begin{equation}
\begin{bmatrix}
x_c\\
y_c\\
1
\end{bmatrix}
= \pi^{-1}
\begin{bmatrix}
x_p\\
y_p\\
1
\end{bmatrix}
\end{equation}

In the depth subfilter, camera poses are treated as known and we estimate the state $x$ which has trivial dynamics $\dot x=0$ assuming the feature is static in space. When a new image arrives at time $t$ to which the same feature is observed at pixel location $x_p(t)\in\real^2$, the feature state is updated using the following measurement model:
\begin{equation}
 x_p(t) = \pi\big(g_{bc}^{-1}(t) g_{sb}^{-1}(t) g_{sc}(t_r)
 \frac{1}{\rho}
\begin{bmatrix}
x_c\\
y_c\\
1
\end{bmatrix}
\big)
\end{equation}

% \section{Feature process}

% When a feature is dropped by the tracker, update all the groups which see the feature.

% When a new image arrives, create a new group (the total number of groups might exceed the limitation, address later), attach the newly created features to the new group. However, since this group is just observed, no features can be used to update its pose. We should create the new group *after we process all the existing features* with the updated pose.

% Once a new group has been created, attach the newly created features to it and initialize the features at nominal distance to the image plane.

% After the measurement update, if there are groups which do not see any features, remove them from the state. Then add the new group to state.


% \section{Evaluation on TUM-VI dataset}

% We compare Corvis~\cite{tsotsosCS15} and our new implementation XIVO against
% \begin{itemize}
%     \item OKVIS of Leutenegger {\it et al.}~\cite{leutenegger2015keyframe}
%     \item ROVIO of Bloesch {\it et al.}~\cite{bloesch2015robust,bloesch2017iterated}
%     \item VINS of Qin {\it et al.}~\cite{qin2018vins}
% \end{itemize}
% which have been benchmarked on the TUM-VI dataset \cite{schubert2018tum}. Corvis, XIVO and ROVIO are EKF-based, whereas OKVIS and VINS perform keyframe-based nonlinear optimization.

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % EVALUATION WITH TUM RGB-D BENCHMARK SCRIPT 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% \begin{table}[h]
% \begin{center}
% \begin{tabular}{c|c|c|c||c|c|c}
%     \hline
%      \multicolumn{2}{c|}{} & \multicolumn{2}{c||}{Keyframe-based} & \multicolumn{3}{c}{EKF-based} \\
%     \hline
%     Sequence & length & OKVIS & VINS & ROVIO & Corvis & XIVO \\
%     \hline\hline
%     room1 & 156{\em m} & \textbf{0.06}{\em m} & 0.07{\em m} & 0.16{\em m}& 0.10{\em m} & 0.22{\em m} \\

%     room2 & 142{\em m} & 0.11{\em m} & \textbf{0.07}{\em m} & 0.33{\em m}& 0.15{\em m} & 0.08{\em m} \\

%     room3 & 135{\em m} & \textbf{0.07}{\em m} & 0.11{\em m} & 0.15{\em m}& 0.12{\em m} &0.11{\em m} \\

%     room4 & 68{\em m} & \textbf{0.03}{\em m} & 0.04{\em m}  & 0.09{\em m}& 0.12{\em m} & 0.08{\em m} \\

%     room5 & 131{\em m} & \textbf{0.07}{\em m} & 0.20{\em m} & 0.12{\em m}& 0.12{\em m} & 0.11{\em m} \\

%     room6 & 67{\em m} & \textbf{0.04}{\em m} & 0.08{\em m}  & 0.05{\em m}& 0.06{\em m} & 0.12{\em m} \\

%     \hline
% \end{tabular}
% \end{center}
% \caption{\textit{RMSE ATE} in meters.}
% \end{table}

% \begin{table}[h]
% \begin{center}
% \begin{tabular}{c|c|c||c|c|c}
%     \hline
%       & \multicolumn{2}{c||}{Keyframe-based} & \multicolumn{3}{c}{EKF-based} \\
%      \hline
%     Sequence & OKVIS & VINS & ROVIO & Corvis & XIVO \\
%     \hline\hline
%     room1 & \textbf{0.013}{\em m} / \textbf{0.43}$\degree$ & 0.015{\em m} / 0.44$\degree$ & 0.029{\em m} / 0.53$\degree$ & 0.047{\em m} / 3.45$\degree$ & 0.031{\em m} / 0.59$\degree$\\

%     room2 & \textbf{0.015}{\em m} / \textbf{0.62}$\degree$ & 0.017{\em m} / 0.63$\degree$ & 0.030{\em m} / 0.67$\degree$ & 0.037{\em m} / 2.56$\degree$ & 0.023{\em m} / 0.75$\degree$\\

%     room3 & \textbf{0.012}{\em m} / \textbf{0.63}$\degree$ & 0.023{\em m} / \textbf{0.63}$\degree$ & 0.027{\em m} / 0.66$\degree$ & 0.033{\em m} / 2.66$\degree$ & 0.027{\em m} / 0.73$\degree$\\

%     room4 & \textbf{0.012}{\em m} / 0.57$\degree$ & 0.015{\em m} / \textbf{0.41}$\degree$ & 0.022{\em m} / 0.61$\degree$ & 0.13{\em m} / 4.80$\degree$& 0.023{\em m} / 0.62$\degree$\\

%     room5 & \textbf{0.012}{\em m} / \textbf{0.47}$\degree$ & 0.026{\em m} / \textbf{0.47}$\degree$ & 0.031{\em m} / 0.60$\degree$ & 0.052{\em m} / 4.59$\degree$& 0.023{\em m} / 0.65$\degree$\\

%     room6 & \textbf{0.012}{\em m} / 0.49$\degree$ & 0.014{\em m} / \textbf{0.44}$\degree$ & 0.019{\em m} / 0.50$\degree$ & 0.020{\em m} / 2.04$\degree$& 0.031{\em m} / 0.53$\degree$\\
%     \hline
% \end{tabular}
% \end{center}
% \caption{\textit{RMSE RPE} on 1 second segments.}
% \end{table}

% \section{Planned features}

% \begin{enumerate}
%     \item Online camera intrinsics calibration.
%     \item Loop closure.
%     \item Hybrid in-state and out-of-state (MSCKF) measurement update.
% \end{enumerate}


\clearpage

\appendix
\section{Skew-symmetric matrix identities}

\begin{prop}
If $R \in\SO{3}, \omega\in\real^3$, then $R\asym{\omega} R^\top=\asym{R\omega}$.
\end{prop}

\begin{proof}
Let $\SO{3} \ni R^\top=[r_1, r_2, r_3]$, then $r_i^\top r_j=\delta_{ij}$ and 

\begin{equation}
\begin{aligned}
r_1 \times r_2=r_3\\
r_2 \times r_3=r_1\\
r_3 \times r_1=r_2\\
\end{aligned}
\label{eq-basis}
\end{equation}

recall $\asym{a} b\doteq a\times b$ and $a\times b = -b\times a$.

\begin{equation}
\begin{aligned}
R\asym{\omega}R^\top
&=
\begin{bmatrix}
r_1^\top\\
r_2^\top\\
r_3^\top
\end{bmatrix}
\asym{\omega}
\begin{bmatrix}
r_1 & r_2 & r_3
\end{bmatrix}\\
&=
\begin{bmatrix}
r_1^\top \asym{\omega}r_1 & r_1^\top \asym{\omega}r_2 & r_1^\top\asym{\omega} r_3 \\
r_2^\top \asym{\omega}r_1 & r_2^\top \asym{\omega}r_2 & r_2^\top\asym{\omega} r_3 \\
r_3^\top \asym{\omega}r_1 & r_3^\top \asym{\omega}r_2 & r_3^\top\asym{\omega} r_3 \\
\end{bmatrix}
\end{aligned}
\end{equation}

Notice $r_i^\top\asym\omega r_i=r_i^\top (\omega \times r_i)=0$ and $\real\ni r_i^\top \asym\omega r_j=\big( r_i^\top \asym\omega r_j \big)^\top=r_j^\top (\asym\omega)^\top r_i=-r_j^\top\asym\omega r_i$ since $\asym\cdot$ is skew-symmetric. Therefore  $R\asym\omega R^\top \in so(3)$ to which we can apply the \textit{vee} operator to bring it to $\real^3$
\begin{equation}
\big(R\asym{\omega}R^\top\big)^\vee
=
\begin{bmatrix}
r_3^\top\asym\omega r_2\\
r_1^\top\asym\omega r_3\\
r_2^\top\asym\omega r_1
\end{bmatrix}
\end{equation}

Now we only need to show $\big(R\asym\omega R^\top\big)^\vee=R\omega=[r_1^\top \omega, r_2^\top \omega, r_3^\top \omega]^\top$. Without loss of generality, let's show the equality holds for the first component, \textit{i.e.}, 
\begin{equation}
\begin{aligned}
& r_3^\top\asym\omega r_2 = r_1^\top\omega \quad \forall \omega \in \real^3\\
\Leftrightarrow  & -r_3^\top \asym{r_2} \omega = r_1^\top \omega \quad \forall \omega \in \real^3 \\
\Leftrightarrow & -r_3^\top \asym{r_2} = r_1^\top \\
\Leftrightarrow & \asym{r_2} r_3 = r_1 \quad \text{ skew-symmetric } \asym\cdot^\top=-\asym\cdot\\
\Leftrightarrow & r_2 \times r_3 = r_1
\end{aligned}
\end{equation}
where last equation holds due to properties of rotation matrics Eq.~\eqref{eq-basis}.
\end{proof}

\begin{prop}
If $a, b\in\real^3$, then $\big(\asym{a}\asym{b}\big)^\top=\asym{b}\asym{a}$.
\end{prop}

\begin{proof}
$\big(\asym{a}\asym{b}\big)^\top=\asym{b}^\top\asym{a}^\top=-\asym{b}\cdot -\asym{a}=\asym{b}\asym{a}$
\end{proof}

\begin{prop}
If $a, b\in\real^3$, then $\asym{(a\times b)} = \asym{a}\asym{b} - \asym{b}\asym{a}$.
\end{prop}
\begin{proof}
    Expand both sides as $3\times 3$ matrices and verify each entry.
\end{proof}

\section{Some transformations and their Jacobians}

Some useful transformation and the Jacobians with respect to the error state of each part involved in the transformation. 

\subsection{Notation}
\begin{itemize}
    \item Rigid body transformation: $g=[R|T]\in\SE{3}, \text{ where } R\in\SO{3}, T\in\real^3$. Error state representation: $R=\bar R(I+\asym{\omega})$, $T=\bar T + \tilde T$
    \item 3D point: $X=\bar X + \tilde X \in \real^3$
\end{itemize}

\subsection{Apply rigid body transformation to a 3D point}

The new point $X_n=gX$, it is easy to see the nominal state and error state representation of $X_n$ is:

$$
\bar X_n = \bar R \bar X + \bar T, \text{ and } \tilde X_n = \bar R\tilde X - \bar R\asym{\bar X}\omega + \tilde T
$$

\subsection{Composition of rigid body transformation}

The composed rigid body motion $g\doteq [R|T] = g_1 g_2=[R_1 R_2|R_1 T_2 + T_1]$

To obtain the error state representation of the rotation, expand both sides of the rotational part of the equation:
\begin{align}
    \bar R (I+\asym{\omega}) &= \bar R_1(I+\asym{\omega_1})\bar R_2(I+\asym{\omega_2}) \\
    \bar R + \bar R\asym{\omega} &= \bar R_1 \bar R_2 + \bar R_1\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2}\quad\text{(drop 2nd-order error term)}
\end{align}

It's easy to see the nominal state is $\bar R = \bar R_1 \bar R_2$, and the error state equation $\bar R\asym{\omega}=\bar R_1\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2}$ requires more work, since the error term $\omega$ is not expressed as a linear combination of $\omega_1, \omega_2$ yet:

\begin{align}
    \bar R \asym{\omega} &= \bar R_1\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2} \\
    \bar R \asym{\omega} &= \bar R_1 \bar R_2 \bar R_2^\top\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2}\quad\text{(insert identity} I\doteq \bar R_2\bar R_2^\top \text{)}\\
    \bar R \asym{\omega} &= \bar R_1 \bar R_2 \asym{\bar R_2^\top\omega_1} + \bar R_1 \bar R_2 \asym{\omega_2}\quad\text{(use identity} R\asym{\omega}R^\top=\asym{R \omega} \text{)} \\
    \bar R \asym{\omega} &= \bar R \asym{\bar R_2^\top\omega_1} + \bar R\asym{\omega_2}\quad\text{(use nominal equation} \bar R=\bar R_1 \bar R_2 \text{)} \\
    \asym{\omega} &= \asym{\bar R_2^\top \omega_1} + \asym{\omega_2} \\
    \omega &= \bar R_2^\top \omega_1 + \omega_2\quad\text{(}\asym{\cdot}\text{ is linear)}
\end{align}

For the translation part:
\begin{align}
\bar T + \tilde T &= \bar R_1(I + \asym{\omega_1})(\bar T_2 + \tilde T_2) + \bar T_1 + \tilde T_1 \\
\bar T + \tilde T &= (\bar R_1 \bar T_2 + \bar T_1) + \bar R_1 \tilde T_2 + \bar R_1 \asym{\omega_1} \bar T_2 + \tilde T_1 \\
\bar T + \tilde T &= (\bar R_1 \bar T_2 + \bar T_1) + \bar R_1 \tilde T_2 - \bar R_1 \asym{\bar T_2} \omega_1 + \tilde T_1,
\end{align}
thus the nominal and error state equations are:
$$
\bar T = \bar R_1 \bar T_2 + \bar T_1 \quad\text{ and } \tilde T = \bar R_1 \tilde T_2 -\bar R_1 \asym{\bar T_2} \omega_1 + \tilde T_1
$$

\subsection{Inverse of rigid body transformation}
The inverse of rigid body transformation $g=[R | T]$ is $g^{-1}\doteq [R_i | T_i]=[R^\top|-R^\top T]$

Error state equation for the rotational part:
\begin{align}
    \bar R_i (I + \asym{\omega_i}) &= \big(\bar R(I + \asym{\omega})\big)^\top \\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top + \asym{\omega}^\top \bar R^\top \\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top - \asym{\omega} \bar R^\top\quad\text{(}\asym{\cdot}\text{ is asymmetric)}\\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top - \bar R^\top \bar R \asym{\omega} \bar R^\top\quad\text{(insert identity} I=\bar R^\top\bar R\text{ )} \\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top - \bar R^\top \asym{\bar R\omega}\quad\text{(use identity} R\asym{\omega}R^\top=\asym{R\omega},
\end{align}

Now the nominal equation is $\bar R_i = \bar R^\top$, and the error state equation is:
\begin{align}
    \bar R_i\asym{\omega_i} &= -\bar R^\top\asym{\bar R\omega} \\
    \bar R^\top \asym{\omega_i} &= -\bar R^\top\asym{\bar R\omega}\quad\text{(since} \bar R_i=\bar R^\top\text{ )} \\
    \asym{\omega_i} &= -\asym{\bar R\omega} \\
    \omega_i &= -\bar R\omega\quad\text{(} \asym{\cdot}\text{ is linear)}.
\end{align}

For the translational part:
\begin{align}
    \bar T_i + \tilde T_i &= -\big(\bar R(I + \asym{\omega})\big)^\top(\bar T + \tilde T) \\
    \bar T_i + \tilde T_i &= (-\bar R^\top + \asym{\omega}\bar R^\top)(\bar T + \tilde T) \\
    \bar T_i + \tilde T_i &= -\bar R^\top \bar T + \asym{\omega}\bar R^\top\bar T -\bar R^\top \tilde T \\
    \bar T_i + \tilde T_i &= -\bar R^\top \bar T - \asym{\bar R^\top\bar T}\omega -\bar R^\top \tilde T,
\end{align}
thus the nominal and error state equations are:
$$
\bar T_i = -\bar R^\top\bar T,\text{ and } \tilde T_i = -\asym{\bar R^\top\bar T}\omega - \bar R^\top \tilde T.
$$

\subsection{General composition rule}
Given a nonlinear function $g_2=f(g_1): \SE{3}\mapsto\SE{3}$, and assume its corresponding error state equation reads
$$
\omega_2 = A \omega_1, \text{ and } \tilde T_2 = B \omega_1 + C \tilde T_1.
$$

Now if we pass $g_2$ to another nonlinear function $g_3=h(g_2):\SE{3}\mapsto\SE{3}$ whose error state equation reads
$$
\omega_3 = D \omega_2, \text{ and } \tilde T_3 = E \omega_2 + F \tilde T_2,
$$

then the error state for $g_3=h(f(g_1)$ reads:
$$
\omega_3 = D A \omega_1, \text{ and } \tilde T_3 = E A \omega_1 + F(B\omega_1 + C\tilde T_1).
$$




\clearpage
\bibliographystyle{unsrt}
\bibliography{refs}

\end{document}
